#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#
# Copyright (C) 2023 reinovo, Inc. All Rights Reserved 
#
# @Time    : 2024/1/10 上午9:33
# @Author  : hmm
# @Email   : liuyuhang0531@foxmail.com
# @File    : apritag_grad.py
# @Software: PyCharm

from arm_control import uarm_control
import cv2
import numpy as np

mtx = np.load('../data/camera_param.npz')['mtx']
with np.load('../data/eyehand_Matrix.npz') as X:
    R, t = [X[i] for i in ('R', 't')]

arm = uarm_control.Arm_controller(mtx,R,t)

print("----------------------")
cap = cv2.VideoCapture(4);
_, frame = cap.read()
# cv2.imshow("frame",frame)
cv2.waitKey(10)
cv2.destroyAllWindows()
cap.release()

tags = arm.tf.get_img2tag(frame, 20)
i = 0
for tag in tags:
    print(tag[3][2])
    print("----------------------")
    if tag[0] == 0:
        pos = arm.tag_point(tag)+[0,0,2]
        arm.gate_move(pos,[200,50,10],50,offset_angle=-tag[3][2])



